#include "SMARTSESSolve.h"

void SMARTSESIterationSolve(double *A, double *B, double *C, double *D, SES_SRC *sesSrc, double **X, double *Y, 
	int n, double steps, double hfactor, FILE *fileHandler)
{
	int solveSteps;
	double solveHfactor;
	int i;
	
	if(steps == -1)
		solveSteps = MAXSTEPS;
	else
		solveSteps = steps;
	if(hfactor == -1)
		solveHfactor = HFACTOR;
	else
		solveHfactor = hfactor;
	
	char* SparseMatrix = NULL;
	FILE *DebugFileHandler = fopen("TranData4Gnuplot.txt","w");
	double *U = SMARTNEW(double, solveSteps);
	SMARTSESWriteTitle(A, B, C, D, sesSrc, n, fileHandler);
	for(i = 0; i < solveSteps; i++)
	{
		SMARTSESUpadateUMatrix(&U, sesSrc, i, n);		
		SMARTSESSolveFormular(&SparseMatrix, A, X, B, U, n, solveHfactor);		
		SMARTSESCalculateY(C, *X, D, U, Y, n);
		SMARTSESWriteToFile(*X, *Y, n, fileHandler);
		DebugSMARTSESStoreData(*X, *Y, n, i, solveHfactor, DebugFileHandler);		
	}
	fclose(DebugFileHandler);
	SMARTDELETE(U);
	//spDestroy(SparseMatrix);
}
//ToDo: to implement it when struct debug starts
void SMARTSESUpadateUMatrix(double **UPoint, SES_SRC *sesSrc, int step, int n)
{
	int i;
	//if(sesSrc != NULL)
	//{
		//ToDo: Parse the parameter class/struct of the excitation
		//Hard Code : Step Wave;
		switch(step)
		{
			case 0:
				for(i = 0; i < n; i++)
					(*UPoint)[i] = 0;
				break;
			default:	
				for(i = 0; i < n; i++)
					(*UPoint)[i] = 1;
				break;
		}
	//}
}

void SMARTSESCalculateY(double *C, double *X, double *D, double *U, double *Y, int n)
{
	int i;
	double Ci, Di, Xi, Ui;
		
	*Y = 0;	
	for(i = 0; i < n; i++)
	{
		Ci = (C == NULL) ? 0 : C[i];
		Di = (D == NULL) ? 0 : D[i];
		Xi = (X == NULL) ? 0 : X[i];
		Ui = (U == NULL) ? 0 : U[i];
			
		*Y += Ci*Xi + Di*Ui;
	}	
}

void SMARTSESSolveFormular(char **SparseMatrix, double *A, double **X, double *B, double *U, int n, double hfactor)
{
	double* RHS = SMARTNEW(double, n);
	double* AMatrix = SMARTNEW(double, n*n);	
	
	SMARTSESConstructAXRHS(A, *X, B, U, &RHS, &AMatrix, n, hfactor);
	SMARTSESSolveAXRHS(AMatrix, X, RHS, n, SparseMatrix);
	
	SMARTDELETE(RHS);
	SMARTDELETE(AMatrix);
}

void SMARTSESConstructAXRHS(double *A, double *X, double *B, double *U, 
		double **RHS, double **AMatrix, int n, double hfactor)
{
	int i,j;
	double Bij, Uij, Xij, Aij, Xi;
	if(*RHS != NULL)
	{
		for(i = 0; i < n; i++)
		{
			double temp = 0; //BU+AX

			Xi = (X == NULL) ? 0 : X[i];
						
			for(j = 0; j < n; j++)
			{	
				Aij = (A == NULL) ? 0 : A[i*n+j];
				Bij = (B == NULL) ? 0 : B[i*n+j];
				Uij = (U == NULL) ? 0 : U[j];				
				Xij = (X == NULL) ? 0 : X[j];
						
				temp += Bij*Uij + 0.5 * Aij*Xij;
			}
			(*RHS)[i] = hfactor * temp + Xi;
		}		
	}

	if(*AMatrix != NULL)
	{		
		for(i = 0; i < n; i++)
		{	
			for(j = 0; j < n; j++)
			{
				Aij = (A == NULL) ? 0 : A[i*n+j];								
				(*AMatrix)[i*n + j] = ((i==j) ? 1 : 0) - 0.5 * hfactor * Aij;
			}
		}
	}
}
//ToDo: Simplify Sparse Solving in the future for AC analysis with better Complex procession
void SMARTSESSolveAXRHS(double *AMatrix, double **X, double *RHS, int n, char **SparseMatrix)
{	
	ComplexNumber *pValue, *pInitInfo, *CheckOutComplexArray();
	RealNumber *pElement;	
	
	double Imag = 0;

	int Error, pError;
	int Count = 0;
	int i,j;

	double *RHSSparse = SMARTNEW(double, n + 1);
	double *XSparse = SMARTNEW(double, n + 1);
	//Sparse CreationFormular
	if(*SparseMatrix == NULL)
	{			
		*SparseMatrix = spCreate(n,0,&Error);
	}
	//Initialize A and RHS in Sparse
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			if(AMatrix[n*i+j] != 0)
			{
				if (Count == 0)
            				{pValue = CheckOutComplexArray(Count = 1+n);}
				pElement = spGetElement(*SparseMatrix,i+1,j+1);
				if (pElement == NULL)
        			{   
					fprintf(stderr, "Insufficient memory available.\n");
            				exit(1);
        			}
				pInitInfo = (ComplexNumber*)spGetInitInfo(pElement);
        			if (pInitInfo == NULL)
       		 		{   
				    pValue[--Count].Real = AMatrix[n*i+j];
	                       pValue[Count].Imag = Imag;            			    
            			    spInstallInitInfo(pElement,(char*)(pValue + Count));
        			}	
			}	
		}		
		RHSSparse[i+1] = RHS[i];
	}	
	spSetReal(*SparseMatrix);
	spInitialize(*SparseMatrix, Init);
	Error = spOrderAndFactor(*SparseMatrix,(RealVector)RHSSparse, 0, 0, 1);
	
	spSolve(*SparseMatrix,(RealVector)RHSSparse,(RealVector)XSparse);

	for (i = 0; i < n; i++)
	{
       	(*X)[i] = XSparse[i+1];		
	}
	
	spClear(*SparseMatrix);

	SMARTDELETE(RHSSparse);		
	SMARTDELETE(XSparse);
}
